package Agent;

import Agent.*;
import Ares.Common.World.*;
import Ares.Common.*;
import Ares.Common.Commands.*;
import Ares.Common.Commands.AgentCommands.*;
import Ares.Common.Commands.AresCommands.*;
import java.util.*;

public class PathFinder
{
	
	// Parent list and gscore list
	private HashMap<Grid, Grid> parents = new HashMap<Grid, Grid>();
	private HashMap<Grid, Integer> gScore = new HashMap<Grid, Integer>();
	private HashMap<Grid, Integer> fScore = new HashMap<Grid, Integer>();
	private Grid destination;
	
	/**
	* This method will be called every time an
    * agent wants to move and it will return the
    * next grid on the "optimal" path.
    * 
    * @param start The current location of the agent.
    * @param goal The next move. 
    */
	public int aStar(Grid start, Grid goal, World world)
    {
		System.out.println("Start location is (r,c): " + start.getLocation().getRow() + ", " + start.getLocation().getCol()); 
		destination = goal;
		
		// Initialize openlist with start location
		ArrayList<Grid> openList = new ArrayList<>();
		openList.add(start);
		
		// Initialize closedlist
		ArrayList<Grid> closedList = new ArrayList<>();
		
		// Initalize gscore of start as 0 and then intialize fScore
		gScore.put(start, new Integer(0));
		fScore.put(start, new Integer(gScore.get(start) + HScore(start)) );
		
		Grid temp, current;
		
		while ( !openList.isEmpty() )
		{
			// Get the grid with lowest fScore in open list
			current = getLowestFScore(openList);
			
			System.out.println("Current grid is (r,c): " + current.getLocation().getRow() + ", " + current.getLocation().getCol());
			
			// If current grid is the goal location we are done
			if (current.getLocation().equals(destination.getLocation()))
			{
				System.out.println("Returning path.");
				return constructPath(start, current);
			}
			
			// Move current location to closed list
			openList.remove(current);
			closedList.add(current);
			
			/* Loop through all adjacent squares and add non killer 
			* squares to openList
			*/
			for(int i = 0; i < 9; i ++)
			{
				Direction dir = Direction.getDirection(i);
				// Skip Unknown and Stay put
				if (dir == Direction.UNKNOWN || dir == Direction.STAY_PUT)
				{
					System.out.println("Skipping direction: " + dir.toString());
					continue;
				}
				// Get neighbour in direction dir
				temp = world.getGridNeighbours(dir, current.getLocation());
				
				// Skip grids off the board
				if (temp == null)
				{
					System.out.println("Grid in the direction: " + dir.toString() + " is null.");
					continue;
				}
				
				// Skip killer grids
				if (temp.isKiller())
					continue;
			
				System.out.println("Grid in the direction: " + dir.toString() + " is open.");
			
				// Skip grids we have already examined
				if ( closedList.contains(temp) )
					continue;
			
				// If not a killer grid add to open list
				if (!openList.contains(temp) )
				{
					openList.add(temp);
					parents.put(temp, current);
				}
				
				// Calculate tentative Gscore
				gScore.put(temp, new Integer( gScore.get(parents.get(temp))) + temp.getMoveCost() );
				int tentativeGScore = gScore.get(current) + HScore(temp);
				
				// Check if it is better to go through current to temp
				// or go straight to temp
				if ( tentativeGScore < gScore.get(temp) )
				{
					// Change parent of temp to current
					parents.put(temp, current);
					// Update G score of temp
					gScore.put(temp, new Integer(tentativeGScore) );
					// Update fscore of temp
					fScore.put( temp, new Integer(gScore.get(temp) + HScore(temp)) );
				}
			}		
		}
		return 5;
	}
	
	/**
	 * Starting from the destination node we go back through parent 
	 * nodes until we reach the start node, then we return the next
	 * node after the start node as the next move.
	 * 
	 * @param start The starting grid
	 * @param destination The Destination Grid
	 * @return Direction to move
	 */
	public int constructPath(Grid start, Grid destination)
	{
		Grid temp = parents.get(destination);
		Grid last = destination;
		
		// Loop through parents
		while ( !temp.getLocation().equals(start.getLocation()))
		{
			last = temp;
			temp = parents.get(temp);
		}
		
		return calculateDirection(temp, last);
	}
	
	/**
	 * Using the directions specs in Direction.java, that is
	 * starting at northwest = 0 and going around clockwise.
	 * 
	 * @param start The start grid
	 * @param dest The destination grid
	 * @return The direction to move based on specification in Direction.java.
	 */
	public int calculateDirection(Grid start, Grid dest)
	{
		int row = start.getLocation().getRow();
		int col = start.getLocation().getCol();
		
		Location destLoc = dest.getLocation();
		
		// Northwest
		if ( destLoc.compareTo( new Location(row-1, col-1)) == 0 )
		{
			return 0;
		}
		// North
		else if ( destLoc.compareTo( new Location(row-1, col)) == 0 )
		{
			return 1;
		}
		// Northeast
		else if ( destLoc.compareTo( new Location(row-1, col+1)) == 0 )
		{
			return 2;
		}
		// East
		else if ( destLoc.compareTo( new Location(row, col+1)) == 0 )
		{
			return 3;
		}
		// Southeast
		else if ( destLoc.compareTo( new Location(row+1, col+1)) == 0 )
		{
			return 4;
		}
		// South
		else if ( destLoc.compareTo( new Location(row +1, col)) == 0 )
		{
			return 5;
		}
		// Southwest
		else if ( destLoc.compareTo( new Location(row+1, col-1)) == 0 )
		{
			return 6;
		}
		// West
		else if ( destLoc.compareTo( new Location(row, col-1)) == 0 )
		{
			return 7;
		}
		else
			return 8;
	}
	
	/**
	 * Returns the grid with the lowest fscore in list.
	 * 
	 * @param list The list of open nodes
	 * @return The grid with the lowest fscore
	 */
	public Grid getLowestFScore(ArrayList<Grid> list)
	{
		Grid ret = list.get(0);
		
		fScore.put(ret, new Integer( HScore(ret) + gScore.get(ret) ));
		
		if (list.size() == 1)
			return ret;
		
		// Loop through list to find grid with smallest f score
		for (Grid grid : list)
		{
			fScore.put(grid, new Integer( HScore(grid) + gScore.get(grid) ));
			if (fScore.get(grid) < fScore.get(ret) )
				ret = grid;
		}
		
		return ret;
	}
	
	/**
	 * Provides an estimation from grid to destination using manhattan
	 * distance.
	 * 
	 * @param grid The grid to calculate from
	 * @return The Hscore.
	 */
	public int HScore(Grid grid)
	{
		int row = Math.abs( grid.getLocation().getRow() - destination.getLocation().getRow());
		int col = Math.abs( grid.getLocation().getCol() - destination.getLocation().getCol());
		return row + col;
	}
}
